// Copyright 1998-2016 Glenn McIntosh
// licensed under the GNU General Public Licence version 3

// include files
#include "quaternion.h"
#include <cmath>
#include <cassert>

namespace math
{
// construct a quaternion from three axis angles
Quaternion::Quaternion(real roll, real pitch, real yaw)
{
	// make quaternion
	real theta = sqrt(roll*roll+pitch*pitch+yaw*yaw);
	real r = theta<1e-10 ? 0.5 : sin(theta/2.)/theta;
	w = cos(theta/2.);
	x = roll * r;
	y = pitch * r;
	z = yaw * r;
}

// construct axis angles from a quaternion
Quaternion::operator Array<3>() const
{
	real theta = acos(w);
	real r = theta<1e-10 ? 2.0 : theta*2/sin(theta);
	return Array<3>{x*r, y*r, z*r};
}

// construct a quaternion from a rotation matrix
Quaternion::Quaternion(const Matrix<3,3> &m)
{
	// convert to unnormalized quaternion
	if (m[0][0] >= 0)
	{
		real m0 = 1.+m[0][0], m12 = m[1][1]+m[2][2];
		if (m12 >= 0)
		{
			w = m0+m12;
			x = m[2][1]-m[1][2];
			y = m[0][2]-m[2][0];
			z = m[1][0]-m[0][1];
		}
		else
		{
			x = m0-m12;
			w = m[2][1]-m[1][2];
			z = m[0][2]+m[2][0];
			y = m[1][0]+m[0][1];
		}
	}
	else
	{
		real m0 = 1.-m[0][0], m12 = m[1][1]-m[2][2];
		if (m12 >= 0)
		{
			y = m0+m12;
			z = m[2][1]+m[1][2];
			w = m[0][2]-m[2][0];
			x = m[1][0]+m[0][1];
		}
		else
		{
			z = m0-m12;
			y = m[2][1]+m[1][2];
			x = m[0][2]+m[2][0];
			w = m[1][0]-m[0][1];
		}
	}
}

// assign a quaternion to a rotation matrix
Quaternion::operator Matrix<3,3>() const
{
	Matrix<3,3> m;

	// calculate normalizing factor
	real tX = x*x, tY = y*y, tZ = z*z;
	real tn = 2./(w*w+tX+tY+tZ);

	// create matrix
	m[0][0] = 1.-(tY+tZ)*tn;
	m[1][1] = 1.-(tX+tZ)*tn;
	m[2][2] = 1-(tX+tY)*tn;
	tX = x*tn;
	tY = y*tn;
	tZ = z*tn;
	real tA, tB;
	tA = x*tY;
	tB = w*tZ;
	m[0][1] = tA-tB;
	m[1][0] = tA+tB;
	tA = x*tZ;
	tB = w*tY,
	m[0][2] = tA+tB;
	m[2][0] = tA-tB;
	tA = y*tZ;
	tB = w*tX;
	m[1][2] = tA-tB;
	m[2][1] = tA+tB;

	// return result
	return m;
}

// convert a quaternion to Euler angles (roll, pitch, yaw)
Array<3> Quaternion::euler() const
{
	Array<3> e;

	// convert to Euler angles
	real q02 = w*w - y*y, q13 = x*x - z*z;
	e[0] = atan2(2.*(y*z+w*x), q02 - q13);
	e[1] = asin(2.*(w*y-x*z));
	e[2] = atan2(2.*(x*y+w*z), q02 + q13);

	// return result
	return e;
}

// multiply a quaternion
Quaternion Quaternion::operator*(const Quaternion &b) const
{
	return Quaternion(
		w*b.w - (x*b.x + y*b.y + z*b.z),
		(w*b.x + x*b.w) + (y*b.z - z*b.y),
		(w*b.y + y*b.w) + (z*b.x - x*b.z),
		(w*b.z + z*b.w) + (x*b.y - y*b.x)
		);
}

// rotate a vector
Array<3> Quaternion::rotate(const Array<3> &b) const
{
	Quaternion r0 = *this * Quaternion(0, b[0], b[1], b[2]) * -*this;
	return Array<3>{r0.x, r0.y, r0.z};
}

// normalize rotation matrix
Matrix<3,3> normalizeRotationMatrix(const Matrix<3,3> &m)
{
	return Quaternion(m);
}
}
